#include "stm32f4xx.h"
#include "rtthread.h"
#include "bsp.h"
#include "drv_st485.h"
#include <string.h>

extern rt_sem_t rt_sem_STUartSendempty;


//start a DMA transmition
//DMA_Streamx:DMA stream,DMA1_Stream0~7/DMA2_Stream0~7 
//ndtr: data send length
void DmaSendDataProc(DMA_Stream_TypeDef *DMA_Streamx,u16 ndtr)
{
	DMA_Cmd(DMA_Streamx, DISABLE);                      //disable DMA transmition 
	while (DMA_GetCmdStatus(DMA_Streamx) != DISABLE){}	//ensure DMA can be set 
	DMA_SetCurrDataCounter(DMA_Streamx,ndtr);          //set data transmition length
	DMA_Cmd(DMA_Streamx, ENABLE);                      // enable DMA transmition
}


/**
  * @name				SendBytesInfoProc
  * @brief			UART 485 DMA send muilt bytes
  * @param  		pSendInfo			u8*    send data address base
							  nSendCount    u16		 send data length
	* @retval 		none
  */
void STUartxDMASendData(u8* pSendInfo, u16 nSendCount)
{
	u16 i = 0;
	u16 j = 0;
	u8 *pBuf = NULL;
	//
	pBuf = ST_SendBuff;//ST_SendBuff is the memory address base of real DMA Uart transmit
 
	for (i=0; i< nSendCount; i++)
	{
		*pBuf++ = pSendInfo[i];
	}
	
	ST_motor485SetTransmit();									//enable 485 tx 
	
	USART_ClearFlag(STMOTOR_UARTx,USART_FLAG_TC);//before start transmit, clear USART TC bit

	DmaSendDataProc(DMA1_Stream6,nSendCount); //start a DMA transmition  
}

void SteeringMotor485WriteCmd(u8 id,u8 reg ,u8 length ,u8* data)
{
	u8 senddata[20];
	u8 cnt_i;
	u32 checksum = 0;
	
	senddata[0] = 0xff;
	senddata[1] = 0xff;//frame header
	senddata[2] = id;
	senddata[3] = length + 3;
	senddata[4] = 0x03;
	senddata[5] = reg;
	for(cnt_i = 0; cnt_i < length ;cnt_i ++)
	{
		*(senddata + 6 + cnt_i) = data[cnt_i];
	}
	for(cnt_i = 2; cnt_i < length + 5 ;cnt_i ++)
	{	
		checksum = checksum + senddata[cnt_i];
	}
	checksum = ~checksum;
	checksum = checksum & 0xff;
	senddata[ 1 + cnt_i] = (u8)checksum;
	
	STUartxDMASendData(senddata, length + 7 );

}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void SteeringMotor485ReadCmd(u8 id,u8 reg ,u8 length )
{
	u8 senddata[8];
	u8 cnt_i;
	u32 checksum = 0;
	
	senddata[0] = 0xff;
	senddata[1] = 0xff;//frame header
	senddata[2] = id;
	senddata[3] = 0x04;	//read cmd ,length is fixed to 4
	senddata[4] = 0x02;	//read cmd ,cmd is fixed to 0x02
	senddata[5] = reg;	
	senddata[6] = length;	

	checksum = senddata[2] + senddata[3]+senddata[4] + senddata[5]+senddata[6];
	checksum = ~checksum;
	checksum = checksum & 0xff;
	senddata[7] = (u8)checksum;	
	
	STUartxDMASendData(senddata, 8 );
	
}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void SetSteeringMotorPositon(u8 acc,u16 speed ,u16 position)
{
	u8 length = 7;
	u8 buff[7];
	
	buff[0] = acc;
	buff[1] = (u8)(position & 0x00ff);
	buff[2] = (u8)((position & 0xff00)>> 8);
	buff[3] =	0x00;
	buff[4] = 0x00;
	buff[5] = (u8)(speed & 0x00ff);
	buff[6] = (u8)((speed & 0xff00)>> 8);
	
	SteeringMotor485WriteCmd(MOTOR_ID, REG_ACC ,length ,buff);	
}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void ReadSteeringMotorPositon(void)
{
		SteeringMotor485ReadCmd(MOTOR_ID,REG_CUR_POS_L ,2 );
}


u16 STMOTOR_RecHandler(u8* recbuff,u8 buff_length)
{
	u32 checksum =0;
	u8 cnt;
	u8 header1,header2,id,datalen,datastatus,crc,data_l,data_h;
	u16 res =0;
	
	header1 = *recbuff;
	header2	= *(recbuff +1);
	id = *(recbuff +2);	
	datalen = *(recbuff +3);
	datastatus = *(recbuff +4);
	crc = *(recbuff + buff_length-1);
	
	data_l = *(recbuff+5);
	data_h = *(recbuff+6);
	
	if (( header1 ==0xff )&&( header2 == 0xff))//frame header 0xff 0xff
	{
		for(cnt = 2;cnt< buff_length-1;cnt++)
			{
				checksum = checksum + *(recbuff+cnt);
			}
			checksum = ~checksum;
			checksum = checksum & 0xFF;
		if(checksum == crc)//chesum pass
		{
			res = (u16)((data_h<<8)+( data_l));
			return res;
		}
	}
	
}

void STUart_SendTask_entry(void* parameter){
	
	u8 data_length = 0;
	s8 i_cnt;
	u8 buff_length;
	
	buff_length = ST_SendLen;//(sizeof(ST_SendBuff)/sizeof(u8));//calculate the buff length
	//ST_SendBuff is the memory address base of real DMA Uart transmit
	while(1){
		
//		for(i_cnt = buff_length-1;i_cnt >=0;i_cnt--){
//			ST_SendBuff[i_cnt] = 0;
//		}
		rt_sem_take(rt_sem_STUartSendempty,RT_WAITING_FOREVER);//wait until STuart send complete empty interrupt occu		
		memset(ST_SendBuff,0,ST_SendLen);//set the buff to value '0' 
		rt_mq_recv(rt_mq_STmotor_send,(void *)&ST_SendBuff,ST_UARTSEND_MSG_SIZE,RT_WAITING_FOREVER);//wait until  STuart send message queue receive
		
		//calculate the uart receive message length
		for(i_cnt = buff_length -1;i_cnt >= 0;i_cnt--){
			if(ST_SendBuff[i_cnt]!=0){
					data_length = i_cnt + 1;
					break;
					}
			else
				data_length = 0;
		}
		
//		memcpy(ST_SendBuff,ST_SendBuff2,ST_SendLen);	//from buff2 copy to buff
			
		ST_motor485SetTransmit();									//enable 485 tx 
		USART_ClearFlag(STMOTOR_UARTx,USART_FLAG_TC);//before start transmit, clear USART TC bit
		DmaSendDataProc(DMA1_Stream6,data_length); //start a DMA transmition  
//			STUartxDMASendData(
		
	}
}

//u8 ST_test_rec_data[9];
//s32 state_code;
//void STUart_RecTask_entry(void * parameter){
//	

//	while(1){
//		
//		state_code = rt_mq_recv(rt_mq_STmotor_rec,ST_test_rec_data,sizeof(ST_test_rec_data),1000);//wait 1000ms
//		if(state_code == -RT_ETIMEOUT)
//			rt_kprintf("receive data overtime!\n");
//		else if(state_code == RT_EOK)
//			rt_kprintf("receive data normal!\n");
//		else
//			rt_kprintf("receive data %ld\n",state_code);
//	}
//}


/**********************************************  END OF FILE drv_st485.c *****************************************************************/
